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Abstract  —  This  paper  proposes  a  Novel  filtering  al¬ 
gorithm  for  the  general  contact  lens  problem,  where  the 
measurement  uncertainty  region  takes  a  thin,  curved, 
contact  lens-like  shape  in  the  states’  Cartesian  coordi¬ 
nates.  Such  problems  have  severe  measurement  nonlin¬ 
earity  and  will  lead  to  consistency  problems  for  exist¬ 
ing  nonlinear  filtering  techniques  such  as  the  extended 
Kalman  filter  (EKF)  and  the  unscented  Kalman  fil¬ 
ter  (UKF).  This  problem  is  very  ill-conditioned,  which 
makes  it  extremely  hard  and  expensive  to  use  a  parti¬ 
cle  filter  (PF).  In  this  paper,  a  General  Measurement 
Adaptive  Covariance  rule  (GMACR)  is  proposed,  for 
which  the  consistency  of  EKF  is  guaranteed.  This  leads 
to  a  new  filtering  approach  for  the  general  contact  lens 
problem  —  the  General  Measurement  Adaptive  Covari¬ 
ance  Extended  Kalman  Filter  (GMAC-EKF).  Simula¬ 
tion  results  show  that  GMAC-EKF  is  consistent  and  has 
superior  tracking  accuracy.  When  the  state  estimate  be¬ 
comes  sufficiently  accurate,  GMAC-EKF  is  equivalent 
to  EKF  and  has  the  optimal  tracking  performance. 

The  only  drawback  of  the  filter  is  that  it  has  loss  in 
accuracy  at  the  early  stage  of  the  filtering  due  to  the 
artificially  enlarged  measurement  covariance.  A  hybrid 
filter  combining  the  alternative  extended  Kalman  filter 
and  GMA  C-EKF  is  also  proposed,  which  yields  the  best 
filtering  performance. 

Keywords:  Tracking,  nonlinear  filtering,  general  con¬ 
tact  lens  problem. 

1  Introduction 

The  Kalman  Filter  (KF)  is  widely  used  for  various 
kinds  of  state  estimation  problems.  It  is  well  known 
that  KF  is  optimal  for  Linear  Gaussian  (LG)  systems. 
For  nonlinear  systems  with  non-Gaussian  noises,  varia¬ 
tions  of  KF,  e.g.,  the  Converted  Measurement  Kalman 
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filter  (CMKF)  [3],  the  Extended  Kalman  Filter  (EKF) 
and  the  Unscented  Kalman  Fitler  (UKF)  have  been 
very  successful.  However  there  are  exceptions,  such  as 
the  very  long  range  tracking  problem  investigated  in 
[11],  which  belongs  to  the  so  called  the  “contact  lens 
problem”  named  after  the  thin,  curved,  contact  lens- 
like  shape  of  the  measurement  uncertainty  region  in  the 
Cartesian  coordinates.  The  curvature  of  the  nonlinear 
measurement  uncertainty  region  and  its  impact  on  var¬ 
ious  filters  was  also  discussed  in  [8,  9].  When  range  rate 
measurements  are  also  involved,  the  problem  becomes 
more  challenging  because  the  contact  lens  issue  exists 
not  only  in  the  position  but  also  in  the  velocity.  The 
problem  is  thus  named  as  the  general  contact  lens  prob¬ 
lem,  which  is  not  covered  by  the  Measurement  Covari¬ 
ance  Adaptive  Extended  Kalman  Filter  (MCAEKF)  in 
[11],  Note  that  nonlinearities  in  state  estimation  prob¬ 
lems  come  from  two  sources:  system  dynamics  nonlin¬ 
earity  and  measurement  nonlinearity.  In  this  paper  we 
focus  on  measurement  nonlinearity,  which  is  directly  re¬ 
lated  to  the  filter  update  and  is  the  prime  cause  of  filter 
inconsistency  and  divergence.1 

The  MCAEKF  is  proposed  for  the  problem  of  very 
long  range  tracking  using  range  and  direction  sine  mea¬ 
surements  from  a  phased  array  radar,  which  is  a  typi¬ 
cal  “contact  lens  problem”.  In  the  presence  of  the  se¬ 
vere  measurement  nonlinearity,  the  filters  above  (except 
CMKF)  failed  to  produce  consistent  filtering  results. 
CMKF,  however,  has  significant  loss  in  range  accuracy. 
MCAEKF  successfully  solved  the  consistency  and  accu¬ 
racy  problems  simultaneously  by  adaptively  changing 
the  measurement  covariance  matrix  to  guarantee  the 
consistency  of  EKF.  However,  it  is  limited  to  the  con¬ 
tact  lens  problem  in  position  and  can  not  be  used  with 
range  rate  measurements. 

An  important  observation  from  MCAEKF  is  that, 
to  guarantee  the  consistency  of  EKF,  the  measurement 


1  The  nonlinearity  in  system  dynamics,  which  is  involved  only 
in  the  track  prediction,  has  been  successfully  treated  in  the  ex¬ 
isting  filtering  approaches,  e.g.,  the  UKF. 
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covariances  need  to  be  modified  to  have  a  less  curved 
uncertainty  region  in  the  Cartesian  coordinates.  In  this 
paper  a  general  measurement  adaptive  covariance  rule 
(GMACR)  is  proposed,  where  the  measurement  is  not 
required  to  be  convertible  to  the  Cartesian  coordinates. 
This  leads  to  a  General  Measurement  Adaptive  Covari¬ 
ance  Extended  Kalman  filter  (GMAC-EKF)  which  is 
shown  to  be  consistent  and  have  superior  tracking  ac¬ 
curacy.  When  the  track  estimate  becomes  sufficiently 
accurate  GMAC-EKF  is  equivalent  to  EKF  and  has  the 
optimal  tracking  performance. 

The  Alternative  Extended  Kalman  Filter  (AEKF)[4, 
7]  was  proposed  for  the  state  update  with  range  rate 
measurements.  It  uses  an  approximate  linearization 
of  the  observation  function  of  the  range  rate  measure¬ 
ment,  which  implicitly  assumes  that  the  position  errors 
do  not  contribute  to  the  errors  of  the  estimated  range 
rate.  AEKF,  although  heuristic,  is  shown  to  be  much 
more  robust  than  EKF  and  has  small  tracking  errors. 
However,  as  shown  later  in  this  paper,  AEKF  has  con¬ 
sistency  problems  as  the  state  estimate  becomes  accu¬ 
rate  and  it  can  cause  significant  loss  in  range  accuracy. 
Further  investigations  indicate  that  it  may  cause  also 
large  errors  in  crossrange.  The  strength  of  AEKF  is  at 
the  initial  stage  of  the  filtering,  when  the  use  of  range 
rate  measurements  will  significantly  improve  the  track¬ 
ing  accuracy  in  the  crossrange  directions.  In  this  pa¬ 
per,  a  hybrid  filter  combining  the  advantage  of  AEKF 
at  the  early  stage  and  the  strength  of  GMAC-EKF  at 
later  stage  of  the  filtering  is  also  proposed,  which  yields 
the  best  filtering  performance. 

The  paper  is  organized  as  follows.  Sec.  2  illustrates 
the  contact  lens  problem  and  shows  the  challenges  in 
solving  it.  The  performance  of  EKF,  UKF,  and  AEKF 
are  investigated  in  a  2-D  tracking  scenario  using  range, 
azimuth  and  range  rate  measurements,  which  show  that 
beyond  certain  limits  these  filters  will  have  consistency 
problems.  The  necessity  of  adaptively  modifying  the 
measurement  covariance  for  EKF  update  is  also  jus¬ 
tified.  Sec.  3  proposes  the  GMACR  and  the  GMAC- 
EKF.  Simulation  results  are  presented  in  Sec.  4.  Con¬ 
cluding  remarks  are  given  in  Sec.  5. 

2  The  General  Contact  Lens 
Problem  and  Its  Challenges 

The  contact  lens  problem  occurs  when  states  in  the 
Cartesian  coordinates  are  updated  with  nonlinear  mea¬ 
surements  from  a  different  coordinate  system  e.g.,  the 
polar  or  the  range-direction-sine  ( r-u-v )  coordinates, 
where  the  measurement  uncertainty  region  has  (because 
of  the  very  accurate  range)  a  very  thin,  curved,  con¬ 
tact  lens-like  shape  in  the  states’  Cartesian  coordinates. 
Fig.  2  shows  an  illustrative  example  of  the  uncertainty 
region  of  such  a  measurement. 

From  basic  geometry,  the  crossrange  uncertainty  in¬ 
creases  with  a  larger  range.  However,  the  uncertainty  in 


Figure  1:  Example:  A  curved  measurement  uncertainty 
region  which  is  very  accurate  in  the  range  direction,  but 
has  large  uncertainty  in  the  crossrange. 

the  range  direction  is  fixed.  As  the  range  increases,  the 
measurement  uncertainty  region  takes  a  curved  shape 
like  a  contact  lens  which  is  increasingly  non-Gaussian 
(non-elliptical)  in  the  Cartesian  coordinates.  When 
the  range  of  such  measurements  is  over  certain  limit, 
conventional  filters,  such  as  EKF,  UKF,  have  been  ob¬ 
served  to  have  significant  consistency  problems  [9,  11]. 
The  CMKF  is  consistent  but  it  has  a  loss  in  range  ac¬ 
curacy  [11].  This  is  because,  to  be  consistent,  the  mea¬ 
surement  conversion  needs  to  significantly  decrease  the 
accuracy  in  the  range  direction.2  The  Particle  filter 
(PF)  is  not  suitable  for  the  contact  lens  problem  be¬ 
cause  of  the  thiness  of  the  measurement  uncertainty 
region,  which  will  cause  severe  particle  depletion. 

Range  rate  measurements  are  often  available  from 
radars.  Like  for  range  measurements,  the  measurement 
nonlinearity  becomes  more  severe  as  the  range  rate  ac¬ 
curacy  increases  and  it  can  also  cause  consistency  prob¬ 
lems  for  EKF  and  UKF.  To  show  this,  consider  a  2-D 
tracking  scenario  where  the  radar  obtains  range  r,  az¬ 
imuth  a  and  range  rate  f  measurements  of  a  target.  The 
target  follows  the  continuous  white  noise  acceleration 
(CWNA)  motion  model  [3]  with  process  noise  intensity 
(PSD)  q.  The  state  of  the  target  is  defined  as 

*  =  [£  £  C  C]'  (i) 

which  evolves  as 

x(k  +  1)  =  Fx(k)  +  v(k)  (2) 

where  T  is  the  sampling  time  and 
'  1  TOO' 
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2See  [11]  for  an  example  of  this  in  the  measurement  conversion 
from  the  r-u-v  coordinates  to  the  Cartesian  coordinates. 


The  covariance  of  the  process  noise  in  (2)  is 


45 


E[v(k)v(k)'] 


iT3  iT2  o  o 

|t2  T  0  0 

0  0  iT3  \T2 

o  o  |t2  t 


q 


(4) 


The  nonlinear  measurements  functions  are  given  by 


r  = 

a  = 

r  = 

The  measurement  noises  wr,  wa  and  w r  are  assumed  to 
be  zero  mean  white  Gaussian  with  standard  deviations 
oy ,  a a  and  ay  respectively,  and  for  the  sake  of  simplicity 
it  is  assumed  that  the  noises  are  mutually  uncorrelated. 

Assume  that  the  sensor  is  at  the  origin  which  takes 
measurements  of  the  target  every  T  =  1  s,  with  mea¬ 
surement  standard  deviations  oy  =  10  m,  <ja  =  2  mrad 
and  oy  to  be  specified.  The  target  has  initial  velocity 
[—9  —2]  m/s  and  process  noise  intensity  q  =  10-3  m2/s3. 
The  initial  position  is  to  be  specified  in  the  simulations. 

In  the  first  case,  EKF  (1st  order)  is  used  for  track¬ 
ing.  Fig.  2  shows  the  filtering  results  including  the  Root 
Mean  Square  position  Error  (RMSposErr)  and  the  Nor¬ 
malized  Estimation  Error  Squared  (NEES)  [3],  which 
are  obtained  using  100  Monte  Carlo  runs.  The  target 
starts  from  [100  100]  km.  Fig.  2  shows  that,  when  the 
standard  deviation  of  the  range  rate  (sdRr)  measure¬ 
ments  is  ay  =  1  m/s,  the  EKF  already  shows  inconsis¬ 
tency  and  has  large  position  errors.  When  the  error 
standard  deviation  decreases  to  ay  =  0.5  m/s,  the  con¬ 
sistency  problem  became  much  worse,  which  leads  to 
the  larger  errors  in  position. 

For  the  UKF,  the  same  tracking  scenario  is  used, 
the  target  starts  further  away  from  the  radar  at 
[600  600]  km.3  From  Fig.  3  it  can  be  seen  that  the  UKF 
has  consistency  problems  when  oy  =  1  m/s.  The  filter 
inconsistency  becomes  more  significant  as  ay  decreases 
to  0.5  m/s.  In  both  cases,  the  UKF  has  loss  in  posi¬ 
tion  accuracy  compared  to  the  UKF  without  the  range 
rate  measurements.  The  reason,  similar  to  that  in  the 
contact  lens  problem  discussed  earlier  in  this  section, 
is  that  in  the  filter  update  the  highly  accurate  range 
rate  measurement  is  overly  “selective”,  like  the  PF,  “ne¬ 
glecting”  the  crossrange  position  estimates. 

Another  filter  using  the  range,  azimuth  and  range 
rate  measurements  is  the  Alternative  Extended  Kalman 
filter  (AEKF)  proposed  in  [4],  For  range  and  azimuth  it 
uses  the  converted  measurement  approach  as  in  CMKF ; 
for  the  range  rate  measurement,  an  approximate  lin- 

3For  the  radar  accuracy  considered,  the  UKF  has  no  consis¬ 
tency  problem  below  this  range. 
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Figure  2:  Case  1:  EKF  filtering  results  for  various  range 
rate  accuracies 
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Figure  3:  Case  2:  UKF  filtering  results  for  various  range 
rate  accuracies 


earization  of  the  observation  matrix  is  used.  The  re¬ 
sulting  observation  matrix  is 
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In  comparison,  the  standard  observation  matrix  is 
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Next,  the  AEKF  is  compared  to  the  UKF  in  the  same 
2-D  tracking  scenario  as  above. 
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Figure  6:  AEKF  vs.  UKF:  range  accuracy 


- UKF  without  range  rate  measurement 

.  AEKF  with  sdRr=0.5m/s 


Sensor: 

sdR=10m 

sdAz=2mrad 

Target  initial  postion: 
[600  600]  km 


50  100 

Time  (s) 


Figure  4:  AEKF  vs.  UKF:  consistency  test 
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Figure  5:  AEKF  vs.  UKF:  RMS  position  errors 

Fig.  4  shows  that  AEKF  shows  some  degree  of  incon¬ 
sistency.  In  contrast,  a  standard  EKF  will  quickly  di¬ 
verge  in  the  same  tracking  scenario,  which  is  not  shown 
in  the  figure  for  the  sake  of  clarity.  The  key  for  AEKF 
to  be  much  more  robust  than  EKF  is  that  the  cross¬ 
correlation  between  the  predicted  state  estimates  and 
the  innovation  is  calculated  using  P(k\k  —  1  )Ha(k)  in¬ 
stead  of  P(k\k  —  l)H(k).  This  effectively  prevents  the 


range  rate  measurement  from  being  “overly  selective” 
on  the  position  estimates,  and  improves  the  consistency 
of  the  filter.  However,  as  shown  in  Fig.  6,  the  heuristic 
AEKF  has  significant  loss  in  range  accuracy  (which  is 
even  worse  than  the  UKF  without  using  the  range  rate 
measurements) .  Further  simulations  show  that  the  con¬ 
sistency  problem  of  AEKF  may  lead  to  large  errors  in 
crossrange. 

Fig.  5  shows  that  the  AEKF  has  smaller  position  er¬ 
rors  than  the  UKF  (without  using  range  rate  measure¬ 
ments)4  before  70s.  AEKF’s  fast  convergence  at  the 
initial  stage  of  the  filtering  is  consistently  observed  in 
various  simulations.  This  is  because  the  use  of  range 
rate  measurements  helps  reduce  the  crossrange  errors 
in  both  velocity  and  position. 

From  the  above  discussions,  for  the  general  contact 
lens  problem,  the  EKF,  UKF  and  AEKF  all  have  their 
working  limits  beyond  which  they  will  show  consistency 
problems  and  have  loss  in  accuracy.  In  the  next  sec¬ 
tion  the  GMACR,  which  guarantees  the  consistency  of 
the  linearized  EKF,  is  presented.  This  is  then  used  to 
develop  the  GMAC-EKF  for  the  general  contact  lens 
problem. 

3  The  Solution  to  the  General 
Contact  Lens  Problem: 
GMAC-EKF 

In  [11]  it  has  been  shown  that  EKF  works  very  well 
when  the  state  estimate  becomes  accurate,  and  unlike 
CMKF,  a  consistent  EKF  has  no  loss  in  accuracy.  In 
fact,  the  consistency  problem  of  EKF  occurs  only  in 
the  early  stage  of  the  filtering  when  the  state  estimates 
are  not  accurate  enough.  Fig.  7  gives  the  geometric 
explanation  for  this  phenomenon. 

4As  shown  in  Fig.  3,  in  the  tracking  scenario  considered,  UKF 
with  the  range  rate  measurements  has  consistency  problems  and 
position  errors  larger  than  the  UKF  without  the  range  rate  mea¬ 
surements. 


In  Fig.  7(a)  the  intersection  of  the  measurement  un¬ 
certainty  and  the  predicted  state  uncertainty,  indicated 
by  the  dark  region,  is  significantly  curved  and  non- 
Gaussian  (non-elliptical).  In  this  case,  the  actual  pos¬ 
terior  uncertainty  of  the  state  will  also  be  very  different 
from  Gaussian,  which  is,  thus,  impossible  to  be  repre¬ 
sented  by  a  Gaussian  density.  For  the  above  reason, 
in  order  to  keep  the  “Gaussian  based”  filters,  e.g.,  the 
EKF  and  UKF,  consistent,  it  is  necessary  to  somehow 
thicken  the  uncertainty  region  to  make  it  more  Gaus¬ 
sian  like.  The  CMKF  does  this  in  its  measurement 
conversion,  which  significantly  decreases  the  accuracy 
in  the  range  direction  [11].  However,  one  feature  of  the 
linearized  EKF  is  trying  to  preserve  the  measurement 
accuracy  and  this  will  cause  consistency  problems  in 
this  case. 

On  the  other  hand,  if  the  predicted  state  estimate 
is  accurate  enough,  as  shown  in  Fig.  3,  the  intersec¬ 
tion  of  the  uncertainties  is  not  very  curved,  thus  the 
posterior  distribution  of  the  state  can  be  well  approxi¬ 
mated  by  a  Gaussian  density.  In  this  case,  EKF  has  no 
consistency  problem  and  will  yield  the  optimal  filtering 
performance. 

Like  MCAEKF,  the  idea  of  GMAC-EKF  is  to  allow 
EKF  to  thicken  the  posterior  uncertainty  when  neces¬ 
sary  by  artificially  increasing  elements  in  the  measure¬ 
ment  covariance  matrix  before  the  update.  The  general 
Measurement  Adaptive  Covariance  rule  (GMACR)  pre¬ 
sented  in  the  sequel  is  able  to  guarantee  the  consistency 
of  EKF  and  is  applicable  to  both  range  and  range  rate 
measurements . 


(a)  Large  predicted  uncertainty  region  in  the 
crossrange  direction  (initial  stage  of  the  filter¬ 
ing) 


(b)  Small  predicted  uncertainty  region  in  the 
crossrange  direction  (when  the  state  estimates 
become  accurate.) 


3.1  GMACR 

From  the  above  discussion,  it  can  be  seen  that  the 
key  for  the  general  measurement  adaptive  covariance 
rule  (GMACR)  is  to  characterize  and  quantify  the  cur¬ 
vature  of  the  posterior  uncertainty  region.  To  illustrate 
how  this  can  be  done,  Fig.  8  shows  a  curved  uncer¬ 
tainty  region  and  the  Gaussian  uncertainty  with  the 
same  center  and  thickness.  The  impact  of  the  curva- 


Gaussian  (elliptical)  uncertainty 
region  with  the  same  center  and 
thickness 


Figure  8:  The  quantification  of  the  curvature  of  a 
curved  (contact  lens  shaped)  uncertainty  region 

ture  at  a  point  in  the  curved  uncertainty  region  can  be 
characterized  by  the  “distance”  Dq  from  the  point  to 
the  corresponding  Gaussian  uncertainty. 

To  be  specific,  consider  the  previously  investigated 

2-D  tracking  problem.  Let  x  =  [£  £  (  C]  denote  the 
center  of  the  two  uncertainty  regions  (the  elliptical  and 
the  contact  lens).  Next,  the  measures  of  the  curvature 
of  the  uncertainty  regions  are  defined. 


Range  measurement 

For  a  point  x  =  [£  £  £  £]  in  the  curved  uncertainty 
region,  from  simple  geometry  one  has 


re  =  V^+C2 

rp  =  &  +  cc 


(10) 

(ii) 


where  re  is  the  range  at  x,  rp  is  the  range  re  projected 
on  x.  From  simple  geometry  one  has 

Drc(x)  =  {^r-r*  (12) 


Range  rate  measurement 

Suppose  x  contains  the  true  velocity.  As  the  posi¬ 
tion  moves  across  the  uncertainty  region,  the  projected 
range  rate  also  changes.  One  has, 


Figure  7:  A  geometric  illustration  of  the  reason  of  the 
divergence  of  EKF 


$$  +  cc 


(13) 


Ve+c 

Drc(x)  =  \fp~fe\ 


(14) 

(15) 


where  re  is  the  range  rate  at  x,  rp  is  the  range  rate 
being  projected  on  x. 

The  distance  Dq(x)  (where  *  stands  for  r  or  r  for 
range  and  range  rate  respectively)  can  also  be  inter¬ 
preted  as  the  error  introduced  by  the  corresponding 
measurement  nonlinearity.  The  curvature  of  the  un¬ 
certainty  region  can  be  quantified  using 

c*  =  maxxe<s>a{Dc(x)}  (16) 

where  *  stands  for  r  or  r,  $a  denotes  the  predicted  un¬ 
certainty  region  that  contains  most  (100a%,  with  typi¬ 
cal  a  =  0.95)  of  the  prior  probability.  For  the  posterior 
uncertainty  region  to  be  Gaussian  like,  its  curvature, 
(w.r.t.  range  or  range  rate)  should  not  be  higher  than 
a  threshold  C* ,  namely 


<  <  C*  (17) 

Formally,  the  general  measurement  adaptive  covari¬ 
ance  rule  (GMACR)  is  as  follows: 

•  Calculate  the  minimum  allowable  standard  devia¬ 
tion  of  the  measurement  noise  a-™111  from  (17) 

<Cin  =  ^rna x{D*c(x)}  (18) 


•  To  guarantee  the  consistency  of  the  linearized 
EKF,  the  standard  deviation  used  by  the  filter  up¬ 
date  should  not  be  smaller  than  a*1  (19),  which 
is  the  larger  one  between  cr“m  (18)  and  the  actual 
measurement  standard  deviation  cr*. 

af  =  max{cr”lm,er*}  (19) 


3.2  GMAC-EKF 

For  the  problem  of  tracking  using  range  and  range 
rate  measurements,  the  GMAC-EKF  operates  as  fol¬ 
lows: 


•  Generate  N  (e.g.,  N  =  100)  random  samples 
from  the  predicted  distribution  of  the  states,  which 
yields  approximately  the  set  $a. 

•  Calculate  Drc{ x)  (12)  and  Drc(x)  (15)  for  all  the 
samples. 


•  Find  <7™m  and  cr™ln  according  to  (18).  Based 
on  GMACR  o^  =  max-fcT™11,  ay}  and  of1  — 
max{cr™m,  cry.}.  Then  one  has  the  modified  mea¬ 
surement  covariance  matrix  as 


Rm 


am)2 


KM)2 


•  Do  the  measurement  update  using  EKF  with  the 
modified  measurement  covariance  matrix  (20). 

Note  that  the  N  samples  can  be  generated  more  effi¬ 
ciently  at  more  desirable  locations,  which  is  illustrated 
in  the  appendix. 

4  Simulation  Results 

The  performance  of  GMAC-EKF  is  evaluated  using 
the  same  tracking  scenario  as  in  Sec.  2.  The  target 
starts  from  [600  600]  km;  the  sensor  accuracies  in  range, 
azimuth,  and  range  rate  are  ay  =  1  m,  cra  =  2  mrad  and 
ay  =  0.5  m/s.  The  threshold  (see  (17))  for  range  and 
range  rate  are  chosen  as5 

Cr  =  &  =  *  (21) 

Fig.  9  shows  the  modified  range  and  range  rate  stan¬ 
dard  deviations  cr)vf  and  of1  (19)  from  the  first  30  s  in 
one  run  of  simulation.  At  the  initial  stage  of  the  fil¬ 
tering  large  standard  deviations  were  used  because  the 
state  estimates  were  very  inaccurate  in  crossrange.  This 
may  temporarily  cause  some  loss  of  tracking  accuracy, 
however,  as  shown  in  Sec.  3,  this  is  necessary  for  the 
linearized  EKF  to  be  consistent.  The  modified  stan¬ 
dard  deviations  decease  sharply  as  the  accuracy  of  the 
state  estimate  increases.  After  12  s  the  actual  range 
and  range  rate  measurement  standard  deviations  are 
used  for  the  filtering  and  GMAC-EKF  is  equivalent  to 
the  standard  EKF  and  its  performance  is  practically 
optimal. 

The  following  simulation  results  (obtained  from  100 
Monte  Carlo  runs)  compare  the  performance  of  GMAC- 
EKF,  UKF  and  AEKF.  Fig.  10  shows  that  the  GMAC- 
EKF  is  consistent,  while  the  other  two  filters  show  dif¬ 
ferent  levels  of  inconsistency.  Fig.  11  shows  that,  at  the 
early  stage  of  the  filtering  (before  30  s),  the  AEKF  has 
the  smallest  position  errors.  This  is  because  that  the 
GMAC-EKF  uses  artificially  increased  standard  devi¬ 
ations  for  the  range  rate  measurements  (see  Fig.  4), 
which  causes  the  filter  to  converge  slower  than  the 
AEKF.  Note  that  the  use  of  the  increased  range  stan¬ 
dard  deviations  causes  little  loss  of  accuracy  in  cross¬ 
range.  After  30  s,  the  GMAC-EKF  has  the  smallest 
position  errors,  because  it  is  consistent  and  equivalent 
to  the  EKF,  thus  has  no  loss  in  accuracy.  Fig.  12  com¬ 
pares  the  accuracy  of  the  filters  in  range,  before  20  s 
the  GMAC-EKF  has  some  loss  in  range  accuracy  due 
to  the  increased  measurement  standard  deviations  in 
range,  after  that  it  has  high  accuracy  as  the  UKF.  The 
AEKF,  however,  has  much  larger  range  errors. 

From  the  above  discussions  it  can  be  seen  that  the 
only  drawback  of  GMAC-EKF  is  the  loss  of  accuracy 
due  to  the  enlarged  measurement  covariance  at  the 

5  For  a  different  problem  cs  need  to  be  fine  tuned  in  the  filter 
design  process. 


(20) 


1400 


(a)  Modified  range  standard  deviation 


Time  (s) 


Figure  11:  GMAC-EKF  vs.  UKF  and  AEKF:  Position 
Errors 


Figure  12:  GMAC-EKF  vs.  UKF  and  AEKF:  range 
accuracy 


(b)  Modified  range  rate  standard  deviation 

Figure  9:  GMAC-EKF:  modified  standard  deviations 
in  range  and  range  rate  from  the  first  30  s  of  one  round 
of  the  simulations. 


early  stage  of  the  filtering.  While  AEKF,  although  not 
performing  very  well  in  the  late  stage  of  the  filtering, 
has  the  strength  of  fast  convergence  in  the  early  filter¬ 
ing  stage.  Note  that  the  GMACR  allows  the  switching 
from  an  arbitrary  filter  to  EKF,  thus  combine  the  ad¬ 
vantages  of  the  filters.  A  simple  switching  rule 


•  Use  AEKF  if  cr™ln  >  ay,  where  cr"lln  is  given  in 
(18);  otherwise,  use  GMAC-EKF. 


Figure  10:  GMAC-EKF  vs.  UKF  and  AEKF:  Consis¬ 
tency  Test 


can  be  used  to  combine  AEKF  and  GMAC-EKF.  The 
resulting  hybrid  filter  yields  the  best  filtering  results, 
which  is  not  shown  in  the  paper  for  the  sake  of  concise¬ 
ness. 

5  Conclusions 

This  paper  investigates  the  problem  of  tracking  in  the 
presence  of  severe  measurement  nonlinearity,  which  is 
referred  to  as  the  general  contact  lens  problem  because 
of  the  thin,  curved  and  contact  lens-like  shape  of  mea¬ 
surement  uncertainty  region.  One  typical  problem  of 
this  kind  is  the  problem  of  tracking  using  highly  accu¬ 
rate  range  and  range  rate  measurements.  For  this  prob¬ 
lem,  existing  nonlinear  filters  including  the  extended 
Kalman  filter  (EKF),  the  unscented  Kalman  (UKF)  fil¬ 
ter  and  the  particle  filter  (PF)  are  shown  to  have  con¬ 
sistency  problems.  The  alternative  extended  Kalman 


filter  (AEKF)  for  the  fusion  of  range  rate  measurements 
is  also  investigated.  It  is  shown  that  AEKF  converges 
very  fast  at  the  early  stage  of  the  filtering,  however,  it 
does  not  perform  very  well  as  the  state  estimate  be¬ 
comes  more  accurate. 

For  the  general  contact  lens  problem,  a  General 
Adaptive  Covariance  Rule  is  proposed,  which  guaran¬ 
tees  the  consistency  of  the  linearized  EKF.  This  leads 
to  the  General  Adaptive  Covariance  Extended  Kalman 
Filter  (GMAC-EKF),  which  is  shown  to  be  consistent 
and  has  the  optimal  performance  when  it  converges  to 
EKF.  A  drawback  of  GMAC-EKF  is  that  it  may  have 
some  loss  in  accuracy  at  the  initial  stage  of  the  filter¬ 
ing  due  to  the  artificially  enlarged  measurement  covari¬ 
ance  matrix.  To  fix  this,  a  hybrid  filter  based  on  the 
GMACR,  which  combines  the  strength  of  AEKF  and 
GMAC-EKF,  can  be  used. 

This  paper  answers  the  key  question  of  how  the  lin¬ 
earized  EKF  can  be  used  for  nonlinear  measurement 
update  without  causing  consistency  problems,  which 
greatly  extends  the  usability  of  EKF  in  nonlinear  fil¬ 
tering  problems. 

6  Appendix  —  An  Efficient  Way 
for  Generating  Samples  for  the 
<f>a  Uncertainty  Region 

Let  xp  and  Pp  denote  the  predicted  position  and 
its  covariance  (Note  that  for  the  problem  considered 
velocity  states  are  not  needed  for  the  calculation  of  Dc 
from  (12)  and  (15).).  The  sample  points  are  generated 
as  follows: 

•  Find  the  orthogonal  basis  of  the  hyperplane  that 
is  perpendicular  to  ip.  (suppose  otp  is  in  a  space 
with  dimension  L,  then  the  dimension  of  the  hy¬ 
perplane  is  L  —  1.) 

•  Generate  N  random  samples  ns(i),i  —  1,  ...,N  in 
this  hyperplane. 

•  For  each  nB(i)  do 

r(i)  =  ria(i),Pp1na(i)  (22) 

na(i)  =  T7^-ris{i)  (23) 

T{l) 

Then  the  samples  are  given  by  xa(i)  =  xp  + 
ns{i),  i  =  1  r-.,  iV. 

The  above  sampling  procedure  will  put  the  samples  at 
the  cross  range  boundary  of  the  $a  uncertainty  region, 
where  the  impact  of  the  nonlinearity  is  the  most  signif¬ 
icant.  For  the  special  case  when  L=2,  there  are  only 
two  such  xa(i)  which  can  be  directly  calculated  without 
using  random  samples. 
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